import os
import sys
from datetime import datetime

import matplotlib.pyplot as plt
import simplekml
from PyQt5.QtCore import Qt, QThread, pyqtSignal
from PyQt5.QtGui import QCloseEvent, QColor
from PyQt5.QtWidgets import QApplication, QMainWindow, QFileDialog, QColorDialog

from GnssLogUtils import PositionPlot
from ui_plot_pos import Ui_PositionPlot

# reference: http://kml4earth.appspot.com/icons.html


from NavParser import NavParser


class KmlThread(QThread):
    logsignal = pyqtSignal(str)
    finished_signal = pyqtSignal(int)

    def __init__(self, column_index, filepath: str, pnt_color: QColor, tck_color: QColor, skip_rows=0, point_hz=100,
                 shape="shaped_dot",
                 enable_different_colors=False
                 ):
        super(KmlThread, self).__init__()
        self.file_path = filepath
        self.skip_rows = skip_rows
        self.column_index = column_index
        self.point_hz = point_hz
        self.pnt_color = pnt_color
        self.tck_color = tck_color
        self.shape = shape
        self.enable_different_colors = enable_different_colors

    def run(self) -> None:
        rgb = self.pnt_color.getRgb()
        pnt_color_str = "%02X%02X%02X%02X" % (rgb[3], rgb[2], rgb[1], rgb[0])
        rgb = self.tck_color.getRgb()
        trk_color_str = "%02X%02X%02X%02X" % (rgb[3], rgb[2], rgb[1], rgb[0])
        parser = NavParser()
        parser.load_from_custom_txt(self.file_path, self.column_index, self.skip_rows)
        parser.to_kml(self.file_path + ".kml", point_color=pnt_color_str, track_color=trk_color_str,
                      down_sample=self.point_hz, shape=self.shape,
                      enable_different_color=self.enable_different_colors
                      )
        self.finished_signal.emit(0)
        return

    # def run(self) -> None:
    #     file_path = self.file_path
    #     skip_rows = self.skip_rows
    #     point_hz = self.point_hz
    #     index = self.column_index
    #     if index[2] < 0 or index[3] < 0 or index[4] < 0:
    #         self.__send_log("position indexes is not correct")
    #         return
    #     rgb = self.tck_color.getRgb()
    #     trk_color_str = "%02X%02X%02X%02X" % (rgb[3], rgb[2], rgb[1], rgb[0])
    #     rgb = self.pnt_color.getRgb()
    #     pnt_color_str = "%02X%02X%02X%02X" % (rgb[3], rgb[2], rgb[1], rgb[0])
    #     kml = simplekml.Kml()
    #     pnints = kml.newfolder(name="Points")
    #     pnt_style = simplekml.Style()
    #     pnt_style.iconstyle.color = pnt_color_str
    #     print(trk_color_str)
    #     print(pnt_color_str)
    #     pnt_style.iconstyle.scale = 0.8
    #     pnt_style.iconstyle.icon.href = shapes_url[
    #         self.shape]  # 'http://maps.google.com/mapfiles/kml/shapes/placemark_circle.png'
    #     fs = point_hz  # 降低频率防止Google Earth崩溃
    #     line_coords = []
    #
    #     with open(file_path, 'r') as f:
    #         for i in range(skip_rows):
    #             f.readline()  # skip header
    #         cnt = 0
    #         while True:
    #             line_data = f.readline()
    #             if line_data == '':
    #                 break
    #             data = line_data.split()
    #             if len(data) < 4:
    #                 self.__send_log("line \" %s \"is skipped." % data)
    #                 continue
    #             try:
    #
    #                 if cnt % fs == 0:
    #                     line_coords.append((float(data[index[3]]), float(data[index[2]])))
    #                     pnt = pnints.newpoint(
    #                         coords=[(float(data[index[3]]), float(data[index[2]]))])
    #                     pnt.style = pnt_style
    #                     pnt.description = "Epoch:%.3f\nLat=%.8f,lon=%.8f,h=%.3f" % (
    #                         float(data[index[1]]), float(data[index[3]]), float(data[index[2]]), float(data[index[4]]))
    #             except Exception as e:
    #                 self.__send_log(e.args[0])
    #             cnt += 1
    #             # self.proc_signal.emit()
    #     lines = kml.newlinestring(name="track", description="", coords=line_coords)
    #     lines.style.linestyle.width = 1
    #     lines.style.linestyle.color = trk_color_str
    #     kml.save("%s.kml" % file_path)
    #     self.finished_signal.emit(0)
    #     return

    def __send_log(self, msg: str):
        self.logsignal.emit(msg)
        return


class PositionPlotWindow(QMainWindow, Ui_PositionPlot):
    def __init__(self):
        super(PositionPlotWindow, self).__init__()
        self.filepath = ""
        self.option = {}
        self.setupUi(self)
        self.btnBrower.clicked.connect(self.onBrowerClicked)
        self.btnStartPlot.clicked.connect(self.onbtnStartPlot)
        self.btnClosePlot.clicked.connect(self.onClosePlot)
        self.btnClearLog.clicked.connect(self.clear_log)
        self.btnExport2Kml.clicked.connect(self.onExportKml)
        self.btn_kml_point_color.clicked.connect(self.on_btn_kml_point_color)
        self.btn_kml_track_color.clicked.connect(self.on_btn_kml_track_color)
        self.set_gui()
        self.column_index = [0, 1, 2, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1]
        self.cur_corr_system = 0  # default is navigation system
        self.cbx_coor_system.currentIndexChanged.connect(self.__On_cbx_coor_changed)
        self.kml_thread = None
        self.dlg = QColorDialog

    def __On_cbx_coor_changed(self):
        print("__On_cbx_coor_changed")
        self.cur_corr_system = self.cbx_coor_system.currentIndex()
        if (self.cur_corr_system == 1):
            self.option["latitude_index"].setText(0, "x-ecef")
            self.option["longitude_index"].setText(0, "y-ecef")
            self.option["height_index"].setText(0, "z-ecef")

        else:
            self.option["latitude_index"].setText(0, "latitude")
            self.option["longitude_index"].setText(0, "longitude")
            self.option["height_index"].setText(0, "height")

    def __get_column_index(self):
        try:
            column_index = [
                int(self.option['gps_week_index'].text(1)),
                int(self.option['gps_second_index'].text(1)),
                int(self.option['latitude_index'].text(1)),
                int(self.option['longitude_index'].text(1)),
                int(self.option['height_index'].text(1)),
                int(self.option['vn_index'].text(1)),
                int(self.option['ve_index'].text(1)),
                int(self.option['vd_index'].text(1)),
                int(self.option['roll_index'].text(1)),
                int(self.option['pitch_index'].text(1)),
                int(self.option['heading_index'].text(1)),
                int(self.option['p_q_index'].text(1)),
                int(self.option['h_q_index'].text(1)),
                int(self.option['ns1_index'].text(1)),
                int(self.option['ns2_index'].text(1)),
            ]
            self.column_index = column_index
        except Exception as e:
            self.column_index = [0, 1, 2, 3, 4, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
            self.show_log(str(e.args))

    def set_gui(self):
        self.option['column_index'] = self.treeWidget.topLevelItem(0)
        self.option['gps_week_index'] = self.treeWidget.topLevelItem(0).child(0)
        self.option['gps_second_index'] = self.treeWidget.topLevelItem(0).child(1)
        self.option['latitude_index'] = self.treeWidget.topLevelItem(0).child(2)
        self.option['longitude_index'] = self.treeWidget.topLevelItem(0).child(3)
        self.option['height_index'] = self.treeWidget.topLevelItem(0).child(4)
        self.option['vn_index'] = self.treeWidget.topLevelItem(0).child(5)
        self.option['ve_index'] = self.treeWidget.topLevelItem(0).child(6)
        self.option['vd_index'] = self.treeWidget.topLevelItem(0).child(7)
        self.option['roll_index'] = self.treeWidget.topLevelItem(0).child(8)
        self.option['pitch_index'] = self.treeWidget.topLevelItem(0).child(9)
        self.option['heading_index'] = self.treeWidget.topLevelItem(0).child(10)

        self.option['p_q_index'] = self.treeWidget.topLevelItem(0).child(11)
        self.option['h_q_index'] = self.treeWidget.topLevelItem(0).child(12)
        self.option['ns1_index'] = self.treeWidget.topLevelItem(0).child(13)
        self.option['ns2_index'] = self.treeWidget.topLevelItem(0).child(14)
        self.option['ref'] = self.treeWidget.topLevelItem(1)
        self.option['ref_lat'] = self.treeWidget.topLevelItem(1).child(0)
        self.option['ref_lon'] = self.treeWidget.topLevelItem(1).child(1)
        self.option['ref_h'] = self.treeWidget.topLevelItem(1).child(2)
        self.option['max_points'] = self.treeWidget.topLevelItem(2)
        self.option['skip_rows'] = self.treeWidget.topLevelItem(3)
        self.option['cdf dense'] = self.treeWidget.topLevelItem(4)
        self.option['kml_down_sample'] = self.treeWidget.topLevelItem(5)
        self.option['start_time'] = self.treeWidget.topLevelItem(6)
        self.option['end_time'] = self.treeWidget.topLevelItem(7)

    def onbtnStartPlot(self):
        filepath = self.editFilePath.text()
        if not os.path.isfile(filepath):
            self.show_log("path is not a file:%s" % filepath)
            return
        self.__get_column_index()
        skip_rows = int(self.option['skip_rows'].text(1))
        ref = None
        if self.option['ref'].checkState(1) == Qt.Checked:
            try:
                ref = [
                    float(self.option['ref_lat'].text(1)),
                    float(self.option['ref_lon'].text(1)),
                    float(self.option['ref_h'].text(1)),
                ]
            except Exception as e:
                self.show_log(e.args[0])
                ref = None
        else:
            ref = None

        max_points = None
        if self.option['max_points'].checkState(1) == Qt.Checked:
            max_points = int(self.option['max_points'].text(1))
            self.show_log('max points is %d' % max_points)
        start_time = -1
        end_time = -1
        start_time = int(self.option["start_time"].text(1))
        end_time = int(self.option["end_time"].text(1))

        bins = None
        if self.option['cdf dense'].checkState(1) == Qt.Checked:
            bins = int(self.option['cdf dense'].text(1))
            self.show_log('cdf dense is %d' % (bins))
        self.cur_corr_system = self.cbx_coor_system.currentIndex()
        s = PositionPlot(filepath, self.column_index, skip_rows=skip_rows, ref=ref, max_points=max_points, bins=bins,
                         coor=self.cur_corr_system)
        if s.exception is not None:
            self.show_log(s.exception.args[0])
            return
        plt.close('all')
        self.show_log("data load finished,%d lines %d columns" % (s.epochs, s.width))
        self.plot_pos_vs_time(s)
        self.plot_pdf_cdf(s)
        self.plot_vel(s)
        self.plot_atti(s)
        self.plot_track(s)
        self.plot_q(s)
        self.plot_ns(s)

    def plot_track(self, s: PositionPlot):
        if s.x_m is not None and s.y_m is not None:
            self.show_log("plot track...")
            plt.figure("track")
            plt.plot(s.x_m, s.y_m)
            plt.xlabel("East/m")
            plt.ylabel("North/m")

    def plot_pos_vs_time(self, s: PositionPlot):
        if s.heights is not None:
            self.show_log("plot pos vs time...")
            plt.figure("pos vs time")
            plt.subplot(3, 1, 1)
            plt.plot(s.times - s.times[0], s.x_m)
            plt.ylabel("east /m")
            # plt.legend(["east"])
            plt.subplot(3, 1, 2)
            plt.plot(s.times - s.times[0], s.y_m)
            plt.ylabel("north /m")
            # plt.legend(["north"])
            plt.subplot(3, 1, 3)
            plt.plot(s.times - s.times[0], s.z_m)
            plt.xlabel("gps second /s - %d" % s.times[0])
            plt.ylabel("height /m")

    def plot_pdf_cdf(self, s: PositionPlot):
        if s.heights is not None:
            self.show_log("plot pdf...")
            plt.figure('pdf and cdf')
            plt.title('pdf and cdf')
            plt.subplot(3, 1, 1)
            plt.step(s.x_edges[1:], s.x_hist / max(s.x_hist), color='r')
            plt.plot(s.x_edges[1:], s.x_cdf / s.x_cdf[-1], color='b')
            plt.legend(['pdf', 'cdf'])
            plt.subplot(3, 1, 2)
            plt.step(s.y_edges[1:], s.y_hist / max(s.y_hist), color='r')
            plt.plot(s.y_edges[1:], s.y_cdf / s.y_cdf[-1], color='b')
            plt.legend(['pdf', 'cdf'])
            plt.subplot(3, 1, 3)
            plt.step(s.z_edges[1:], s.z_hist / max(s.z_hist), color='r')
            plt.plot(s.z_edges[1:], s.z_cdf / s.z_cdf[-1], color='b')
            plt.xlabel('error')
            plt.legend(['pdf', 'cdf'])

    def plot_vel(self, s: PositionPlot):
        if s.vn is not None and s.ve is not None and s.vd is not None:
            self.show_log('plt vel vs time')
            plt.figure('vel vs time')
            plt.subplot(3, 1, 1)
            plt.plot(s.times - s.times[0], s.vn)
            plt.subplot(3, 1, 2)
            plt.plot(s.times - s.times[0], s.ve)
            plt.ylabel('vel /(m/s)')
            plt.subplot(3, 1, 3)
            plt.plot(s.times - s.times[0], s.vd)
            plt.xlabel('gps second /s - %d' % s.times[0])

    def plot_atti(self, s: PositionPlot):
        if s.roll is not None and s.pitch is not None and s.heading is not None:
            self.show_log('attitude  vs time')
            plt.figure('atti vs time')
            plt.subplot(3, 1, 1)
            plt.plot(s.times - s.times[0], s.roll)
            plt.subplot(3, 1, 2)
            plt.plot(s.times - s.times[0], s.pitch)
            plt.ylabel('atti /deg')
            plt.subplot(3, 1, 3)
            plt.plot(s.times - s.times[0], s.heading)
            plt.xlabel('gps second /s - %d' % s.times[0])

    def plot_q(self, s: PositionPlot):
        if s.h_q is not None and s.p_q is not None:
            self.show_log("plot q vs time...")
            plt.figure("q")
            plt.step(s.times - s.times[0], s.p_q, 'bo')
            plt.step(s.times - s.times[0], s.h_q, 'ro')
            plt.legend(['pos q', 'heading q'])
            plt.xlabel('gps second /s - %d' % s.times[0])

    def plot_ns(self, s: PositionPlot):
        if s.ns1 is not None:
            self.show_log("plot satellite number vs time...")
            plt.figure("satellite number")
            plt.title("satellite number")
            plt.step(s.times - s.times[0], s.ns1, color='r')
            plt.legend(['satellite number of antenna 1'])
            if s.ns2 is not None:
                plt.step(s.times - s.times[0], s.ns2, color='b')
                plt.legend(['antenna 1', 'antenna 2'])
            plt.xlabel('gps second /s - %d' % s.times[0])
        plt.show()

    def on_btn_kml_point_color(self):
        c = self.dlg.getColor()
        if c.isValid():
            self.edit_kml_point_color.setText("#%X" % c.rgb())
            print(c.getRgb())
        return

    def on_btn_kml_track_color(self):
        c = self.dlg.getColor()
        if c.isValid():
            self.edit_kml_track_color.setText("#%x" % c.rgb())
            print(c.getRgb())
        return

    def onClosePlot(self):
        plt.close('all')

    def onExportKml(self):
        file_path = self.editFilePath.text()
        skip_rows = int(self.option['skip_rows'].text(1))
        kml_down_sample = int(self.option['kml_down_sample'].text(1))
        self.__get_column_index()
        index = self.column_index

        pnt_color = QColor(self.edit_kml_point_color.text())
        tck_color = QColor(self.edit_kml_track_color.text())
        if not pnt_color.isValid():
            self.show_log("point color is invalid ")
            pnt_color = QColor(255, 255, 0)
        if not tck_color.isValid():
            self.show_log("track color is invalid ")
            tck_color = QColor(255, 255, 0)
        if index[2] < 0 or index[3] < 0 or index[4] < 0:
            self.show_log("index of position is -1")
            return
        if not os.path.isfile(file_path):
            self.show_log("path is not a file:%s" % file_path)
            return
        shapes = ['placemark_circle', 'shaped_dot', 'road_shield3']
        shape = self.comb_shapes.currentIndex()
        enable_different_colors = False
        if self.cbx_enable_different_colors.checkState() == Qt.Checked:
            enable_different_colors = True
        self.show_log("export to %s.kml" % file_path)
        self.kml_thread = KmlThread(self.column_index, file_path, pnt_color, tck_color,
                                    skip_rows=skip_rows, point_hz=kml_down_sample, shape=shapes[shape],
                                    enable_different_colors=enable_different_colors)
        self.kml_thread.logsignal.connect(self.show_log)
        self.kml_thread.finished_signal.connect(self.onExportKmlFinished)
        self.kml_thread.start()
        self.btnExport2Kml.setEnabled(False)

    def onExportKmlFinished(self, x):
        self.show_log("export finished")
        self.btnExport2Kml.setEnabled(True)

    def onBrowerClicked(self):
        filfilepath, suffix = QFileDialog.getOpenFileName()
        self.filepath = filfilepath
        self.editFilePath.setText(filfilepath)
        self.show_log("path: %s" % filfilepath)

    def show_log(self, msg):
        stamp = datetime.now().strftime('%H:%M:%S : ')
        item = stamp + msg
        self.listLog.addItem(item)

    def clear_log(self):
        self.listLog.clear()

    def closeEvent(self, a0: QCloseEvent) -> None:
        plt.close('all')  # 关闭绘图


if __name__ == '__main__':
    app = QApplication(sys.argv)
    w = PositionPlotWindow()
    w.show()
    sys.exit(app.exec_())
